// receives sensor data from simulator, and sends actuator data to simulator

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <visibility.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <common/Thread.h>
#include <common/Runnable.h>
#include <serialport/serialport.h>
#include <serialport/uart.h>
#include "simlink.h"

static Runnable *thread = NULL;

static void simlink_usage(const char *reason);
extern "C" __EXPORT int simlink_thread_main(int argc, char *argv[]);
static SerialPort *createSerialPort(int argc, char *argv[]);


/**
 * The simlink deamon app only briefly exists to start the background job.
 * The stack size assigned in the Makefile does only apply to this management task.
 * The actual stack size should be set in the call to task_spawn().
 */
extern "C" __EXPORT
int simlink_main(int argc, char *argv[]) {

	if (argc < 2)
		simlink_usage("missing command");

	if (!strcmp(argv[1], "start")) {

		if (thread != NULL) {
			printf("simlink already running\n");
			exit(0);
		}

		task_spawn_cmd("simlink", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 3000,
				simlink_thread_main, (const char **)argv);

		exit(0);
	}

	if (!strcmp(argv[1], "stop")) {
		if (thread == NULL)
			printf("\tsimlink is not started\n");
		else {
			thread->setExitFlag();
		}
		exit(0);
	}

	if (!strcmp(argv[1], "status")) {
		if (thread != NULL)
			printf("\tsimlink is running\n");
		else
			printf("\tsimlink is not started\n");
		exit(0);
	}

	simlink_usage("unrecognized command");
	exit(1);
}


static void simlink_usage(const char *reason) {
	if (reason)
		fprintf(stderr, "%s\n", reason);

	errx(1, "usage: simlink {start|stop|status} [-d <devicename>] [-b <baud rate>]\n");
}

extern "C"
int simlink_thread_main(int argc, char *argv[])  {
	SerialPort *port = createSerialPort(argc - 2, argv + 2);

	thread = new SimLink(port);
	thread->run_();
	delete thread;
	thread = NULL;

	delete port;
	return 0;
}


static SerialPort *createSerialPort(int argc, char *argv[]) {
	char *device_name = "/dev/ttyACM0";
	int baudrate = 115200;

	int ch;
	while ((ch = getopt(argc, argv, "b:d:")) != EOF) {
		switch (ch) {
		case 'b':
			baudrate = strtoul(optarg, NULL, 10);

			if (baudrate == 0)
				errx(1, "invalid baud rate '%s'", optarg);

			break;

		case 'd':
			device_name = optarg;
			break;

		default:
			simlink_usage("invalid option");
		}
	}

	return new Uart(device_name, baudrate, 10);
}

